/****************************************************************************
 *
 *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
#include <termios.h>
#include "yt_control.hpp"

#define YT_PITCH    (0)
#define YT_ROLL     (1)
#define YT_YAW      (2)

YtControl::YtControl() :
	ModuleParams(nullptr),
	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1)
{

}

YtControl::~YtControl()
{
	perf_free(_loop_perf);
	perf_free(_loop_interval_perf);
}

bool YtControl::init()
{
	// alternatively, Run on fixed interval
	ScheduleOnInterval(50000_us); // 2000 us interval, 200 Hz rate
	this->OpenSerial("/dev/ttyS3");

	return true;
}

void YtControl::Run()
{
	if (should_exit()) {
		_manual_control_input_sub.unsubscribe();
		exit_and_cleanup();
		return;
	}

	perf_begin(_loop_perf);
	perf_count(_loop_interval_perf);

	const char *data = "ABCEDEF";
	this->SendBytes(data,strlen(data));

	manual_control_setpoint_s manual_control_input;
	if(_manual_control_input_sub.update(&manual_control_input)){
		PX4_INFO("roll = %i", static_cast<int>(manual_control_input.roll*1000));
		PX4_INFO("pitch = %i", static_cast<int>(manual_control_input.pitch*1000));
		PX4_INFO("aux1 = %i", static_cast<int>(manual_control_input.aux1*1000));
		PX4_INFO("aux2 = %i", static_cast<int>(manual_control_input.aux2*1000));
		float pitch_raw = manual_control_input.aux1;
		//float yaw_raw = manual_control_input.aux2;
		uint16_t pitch = 1500 + pitch_raw;
		this->ChangeAngle(YT_PITCH,pitch);
	}


	perf_end(_loop_perf);
}

int YtControl::OpenSerial(const char *dev)
{
	// 波特率为115200
	int rate = B115200;

	// open uart
	int fd = px4_open(dev, O_RDWR | O_NOCTTY);
	int termios_state = -1;

	if (fd < 0) {
		PX4_ERR("failed to open uart device!");
		return -1;
	}

	// set baud rate
	struct termios config;
	tcgetattr(fd, &config);


	config.c_cflag &= ~CSIZE;
	config.c_cflag |= CS8;         // 8数据位
	config.c_cflag &= ~PARENB;     // 无奇偶校验
	config.c_cflag &= ~CSTOPB;     // 1停止位
	config.c_cflag &= ~CRTSCTS;    // 关闭硬件流

	/* Set baud rate */
	if (cfsetispeed(&config, rate) < 0 || cfsetospeed(&config, rate) < 0) {
		PX4_WARN("ERR SET BAUD %s: %d\n", dev, termios_state);
		px4_close(fd);
		return -1;
	}

	if ((termios_state = tcsetattr(fd, TCSANOW, &config)) < 0) {
		PX4_WARN("ERR SET CONF %s\n", dev);
		px4_close(fd);
		return -1;
	}

	this->_fd = fd;

	PX4_INFO("###OPENOK:%i",this->_fd);

	return fd;
}

int YtControl::SendBytes(const void *data, size_t len)
{
	for (size_t i = 0; i < len; i++) {
		PX4_INFO("###FD:%i",this->_fd);
		// Block until we can send a byte
		write(this->_fd, ((const uint8_t *)data) + i, 1);
		//usleep(100);
	}

	return 0;
}
//angle范围：1100 ，1900
void YtControl::ChangeAngle(uint8_t which, uint16_t angle)
{
	static uint8_t buf[7];
	uint16_t tmp;
	uint8_t *s;
	uint8_t num=7;
	buf[0]=0xFA;
	buf[1]=0x02;
	if(which==YT_PITCH){
		buf[2]=0x0B;
	}else if(which==YT_ROLL){
		buf[2]=0x0A;
	}else{
		buf[2]=0x0C;
	}

	buf[3]=(uint8_t)(angle);
	buf[4]=(uint8_t)((angle)>>8);
	tmp=this->do_crc(buf,5);
	buf[5]=(uint8_t)tmp;
	buf[6]=(uint8_t)(tmp>>8);

	s = (uint8_t *) buf;
	PX4_INFO("###CHANGEANgle:===");
	this->SendBytes(s,num);
}

uint16_t YtControl::do_crc(unsigned char * message, int len)
{
	int i, j;
	unsigned short crc_reg;

	crc_reg = (message[0] << 8) + message[1];
	for (i = 0; i < len; i++)
	{
		if (i < len - 2)
		for (j = 0; j <= 7; j++)
		{
			if ((short)crc_reg < 0)
			crc_reg = ((crc_reg << 1) + (message[i + 2] >> (7 - i))) ^ 0x1021;
			else
			crc_reg = (crc_reg << 1) + (message[i + 2] >> (7 - i));
		}
		else
		for (j = 0; j <= 7; j++)
		{
			if ((short)crc_reg < 0)
			crc_reg = (crc_reg << 1) ^ 0x1021;
			else
			crc_reg <<= 1;
		}
	}
	return crc_reg;
}

int YtControl::task_spawn(int argc, char *argv[])
{
	YtControl *instance = new YtControl();

	if (instance) {
		_object.store(instance);
		_task_id = task_id_is_work_queue;

		if (instance->init()) {
			return PX4_OK;
		}

	} else {
		PX4_ERR("alloc failed");
	}

	delete instance;
	_object.store(nullptr);
	_task_id = -1;

	return PX4_ERROR;
}

int YtControl::print_status()
{
	perf_print_counter(_loop_perf);
	perf_print_counter(_loop_interval_perf);
	return 0;
}

int YtControl::custom_command(int argc, char *argv[])
{
	return print_usage("unknown command");
}

int YtControl::print_usage(const char *reason)
{
	if (reason) {
		PX4_WARN("%s\n", reason);
	}

	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description
Example of a simple module running out of a work queue.

)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("work_item_example", "template");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

	return 0;
}

extern "C" __EXPORT int yt_control_main(int argc, char *argv[])
{
	return YtControl::main(argc, argv);
}
